/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#include "stdafx.h"
#include "MotionPlanning_Dyn.h"

namespace RobotPlant { namespace Mechanism{
	using namespace std;
	using namespace RobotPlant::Math;

	MotionSegment_Dyn::MotionSegment_Dyn()
	{
	}

	bool MotionSegment_Dyn::operator == ( const MotionSegment_Dyn& arg )
	{
		return false;
	}

	bool MotionSegment_Dyn::operator < ( const MotionSegment_Dyn& arg )
	{
		return false;
	}

	MotionPlanning_Dyn::MotionPlanning_Dyn(Chain* pRobot, double stepInterval):MotionPlanning(pRobot, stepInterval)
	{
		_motionSegments = std::vector<MotionSegment_Dyn>();
	}

	MotionPlanning_Dyn::~MotionPlanning_Dyn()
	{

	}

	void MotionPlanning_Dyn::MoveForward(Velocity vel, bool global)
	{

	}

	void MotionPlanning_Dyn::MoveJoint(const ColumnVector toJointValues, double speed, double flyByZone)
	{

	}

	void MotionPlanning_Dyn::MoveCart(const Frame3 toPoint, double speed, double flyByZone)
	{

	}

	void MotionPlanning_Dyn::MoveCart_Linear(const Frame3 toPoint, double speed, double flyByZone)
	{

	}

	void MotionPlanning_Dyn::Stop(const bool followPath)
	{

	}

	int MotionPlanning_Dyn::NextStep()
	{
		//define values
		ColumnVector retJointValues = ColumnVector(_pRobot->Get_Dof());
		ColumnVector retJointVels = ColumnVector(_pRobot->Get_Dof());
		ColumnVector retJointAccs = ColumnVector(_pRobot->Get_Dof());
		ColumnVector retJointTorque = ColumnVector(_pRobot->Get_Dof());
		int ret = -1;

		//calculate next step
		//...


		//save next step to current values
		if(ret >= 0)
		{
			SetCurRobotJointValues(retJointValues);
			SetCurRobotJointValues(retJointVels);
			SetCurRobotJointValues(retJointAccs);
			SetCurRobotJointValues(retJointTorque);
		}

		return ret;
	}
}}